#include "stdafx.h"

#include "LocatorPoseMex.h"

void runLocatorPose(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	if (nrhs != 6) mexErrMsgTxt("LocatorPose needs 5 Arguments.");
	if (mxGetN(prhs[1]) != 3) mexErrMsgTxt("xmarker must be an Nx3 matrix.");
	if (mxGetN(prhs[2]) != 3) mexErrMsgTxt("smarker must be an Nx3 matrix.");
	if (mxGetN(prhs[3]) != 3) mexErrMsgTxt("xmodel must be an Nx3 matrix.");
	if (mxGetM(prhs[4]) != 7) mexErrMsgTxt("xlocator0 must be a 7x1 vector");
	
	//mexPrintf("1: %d, 2: %d, 3: %d\n", mxGetM(prhs[1]), mxGetM(prhs[2]), mxGetM(prhs[3]));
	if ( (mxGetM(prhs[1]) != mxGetM(prhs[2])) || (mxGetM(prhs[1]) != mxGetM(prhs[3]) ) )
		mexErrMsgTxt("All input variables must habe the same number of rows.");
		
	arma::mat xmarker = convertMatlabToArma(prhs[1]);		// x_marker
	arma::mat smarker = convertMatlabToArma(prhs[2]);		// s_marker
	arma::mat xmodel = convertMatlabToArma(prhs[3]);		// x_model
	arma::colvec xlocator0 = convertMatlabToArma(prhs[4]);  // x_locator0 -> Anfangswert
	int max_iterations = (int) mxGetPr(prhs[5])[0];			// max_iterations
	
	arma::colvec xlocator;
	arma::colvec slocator;
	bool did_converge;
	

	// OTPF::Locator::locatorPose(xmarker, smarker, xmodel, xlocator, slocator, xlocator0, did_converge, max_iterations); // changed due to new function call in locator.cpp by Gca, 2012-06-11

	arma::colvec f;
	arma::colvec f_temp;
	double err;
	OTPF::Locator::locatorPose(xmarker, smarker, xmodel, xlocator, slocator, xlocator0, did_converge, max_iterations, f, f_temp, err);
	
	plhs[0] = convertArmaToMatlab(xlocator);			// x_locator
	plhs[1] = convertArmaToMatlab(slocator);			// s_locator
	plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);			// did_converge
	plhs[3] = convertArmaToMatlab(f);					// f
	plhs[4] = convertArmaToMatlab(f_temp);				// f_temp
	plhs[5] = mxCreateDoubleMatrix(1,1,mxREAL);			// err


	mxGetPr(plhs[2])[0] = (double) did_converge;
	mxGetPr(plhs[5])[0] = (double) err;
};
